#include <path_planning_study/global_planning_node.h>

// rosrun map_load map_load_node 2F
// rosrun path_planning_study astar_plan_node

// 构造函数
GlobalPlanning::GlobalPlanning():__map_ptr(nullptr){
    // params
    // __path_planning_type = PathPlanningType::JPS;
    __path_planning_type = PathPlanningType::A_STAR;
    __robot_radius = 0.3;
    __output_dir = ros::package::getPath("path_planning_study") + "/output/";
    cout << "__output_dir: " << __output_dir.c_str() << endl;
    InitPlanning();

    // ros
    __path_pub = __nh.advertise<nav_msgs::Path>("/global_path_origin", 1, true);
    __start_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/global_start_pose", 1, true);
    __goal_pose_pub = __nh.advertise<geometry_msgs::PoseStamped>("/global_goal_pose", 1, true);
    __expand_points_pub = __nh.advertise<nav_msgs::OccupancyGrid>("/global_expand_points", 1, true);
    __jps_jump_points_pub = __nh.advertise<nav_msgs::GridCells>("/jps_jump_points", 1, true);
    __map_erode_pub = __nh.advertise<nav_msgs::OccupancyGrid>("/global_map_erode", 1, true);
    __map_sub = __nh.subscribe("/map", 10, &GlobalPlanning::MapCallback, this);
    __rviz_pose_sub = __nh.subscribe("/move_base_simple/goal", 10, &GlobalPlanning::RvizPosesCallback, this);
}

// 析构函数
GlobalPlanning::~GlobalPlanning(){

}

// 地图回调函数
void GlobalPlanning::MapCallback(const nav_msgs::OccupancyGridConstPtr& msg){
    __map_ptr = msg;
    cv::Mat map_image(__map_ptr->info.height, __map_ptr->info.width, CV_8UC1); 
    for (int i = 0; i < __map_ptr->data.size(); ++i) {
        uchar value = static_cast<uchar>(__map_ptr->data[i]);
        if(value == 0){
            map_image.data[i] = 255;
        }
        else if(value == 100){
            map_image.data[i] = 0;
        }
        else
            // map_image.data[i] = 128;
            map_image.data[i] = 0;

    }
    cv::flip(map_image, __map_image, 0);    //上下翻转

    // 障碍物膨胀处理
    int robot_radius_pixel = int(__robot_radius / __map_ptr->info.resolution);
    cv::Mat dilate_struct = cv::getStructuringElement(0, Size(robot_radius_pixel, robot_radius_pixel));     //0 矩形结构元素 1 十字结构元素
    cv::erode(__map_image, __map_image, dilate_struct);
    cv::imwrite(__output_dir + "astar_oigin.png", __map_image);
    ROS_INFO("Receive a map, width=%d, height=%d", __map_ptr->info.width, __map_ptr->info.height);

    nav_msgs::OccupancyGrid map_erode;
    map_erode.header = __map_ptr->header;
    map_erode.header.stamp = ros::Time::now();
    map_erode.info = __map_ptr->info;
    for(int h=0; h<__map_ptr->info.height; h++)
        for(int w=0; w<__map_ptr->info.width; w++){
            int value = __map_image.at<uchar>(__map_ptr->info.height - h, w);
            map_erode.data.emplace_back(value);

            // if( == 255){
            //     map_erode.data.emplace_back(-1);
            // }
            // else{
            //     map_erode.data.emplace_back(100);
            // }
    }
    __map_erode_pub.publish(map_erode);

    // 设置地图数据
    SetMap();
}

// Rviz位姿回调函数
void GlobalPlanning::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    if(!__map_ptr){
        ROS_ERROR("Map is not received");
        return;
    }

    if(__rviz_poses.empty()){
        ROS_INFO("----------------------RvizPoses----------------------");
    }

    // 数量小于2时，追加
    if(__rviz_poses.size() < 2){
        __rviz_poses.emplace_back(*msg);
        ROS_INFO("Add a pose, x=%0.3f, y=%0.3f, yaw=%0.3f. rviz_poses size=%ld", 
            msg->pose.position.x, 
            msg->pose.position.y,
            tf2::getYaw(msg->pose.orientation),
            __rviz_poses.size()
        );
    }

    if(__rviz_poses.size() == 2){
        ROS_INFO("Start planning path");
        __start_pose_pub.publish(__rviz_poses.front());
        __goal_pose_pub.publish(__rviz_poses.back());

        // 开始规划路径
        switch(__path_planning_type){
            case PathPlanningType::A_STAR:
                AstarPath(__rviz_poses.front().pose, __rviz_poses.back().pose);
                break;
            case PathPlanningType::JPS:
                JPSPath(__rviz_poses.front().pose, __rviz_poses.back().pose);
                break;
            default:
                ROS_ERROR("Unknown path planning type");
        }

        // 规划完成，清空位姿
        vector<geometry_msgs::PoseStamped> rviz_poses;
        __rviz_poses.swap(rviz_poses);

    }
}

/******************************************************
 * 主函数 * 
 ******************************************************/
int main(int argc, char** argv){
    ros::init(argc, argv, "global_planning_node");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
	
    ROS_INFO("global_planning_node initializating......");
    ros::NodeHandle nh;
    GlobalPlanning global_planning;
    ros::spin();
    return 0;
}